#!/usr/bin/env python
PACKAGE = "mpc_follower"

from dynamic_reconfigure.parameter_generator_catkin import *


gen = ParameterGenerator()

# -- path smoothing --
gen.add("enable_path_smoothing", bool_t, 0, "flag for path smoothing", False)
gen.add("path_filter_moving_ave_num", int_t, 0, "param of moving average filter for path smoothing", 25, 1, 200)
gen.add("curvature_smoothing_num", int_t, 0, "point-to-point index distance used in curvature calculation : curvature is calculated from three points p(i-num), p(i), p(i+num)", 15, 1, 200)

# -- mpc optimization --
gen.add("mpc_prediction_horizon", int_t, 0, "prediction horizon step", 0, 0, 500)
gen.add("mpc_prediction_dt", double_t, 0, "prediction horizon period [s]", 0.0, 0.0, 1.0)
gen.add("mpc_weight_lat_error", double_t, 0, "lateral error weight in matrix Q", 0.0, 0.0, 10.)
gen.add("mpc_weight_heading_error", double_t, 0, "heading error weight in matrix Q", 0.0, 0.0, 10.0)
gen.add("mpc_weight_heading_error_squared_vel", double_t, 0, "heading error * velocity weight in matrix Q", 0.0, 0.0, 10.0)
gen.add("mpc_weight_steering_input", double_t, 0, "steering error weight in matrix R", 0.0, 0.0, 10.0)
gen.add("mpc_weight_steering_input_squared_vel", double_t, 0, "steering error * velocity weight in matrix R", 0.0, 0.0, 10.0)
gen.add("mpc_weight_lat_jerk", double_t, 0, "lateral jerk weight in matrix R", 0.0, 0.0, 10.0)
gen.add("mpc_weight_steer_rate", double_t, 0, "steering rate weight in matrix R", 0.0, 0.0, 10.0)
gen.add("mpc_weight_steer_acc", double_t, 0, "steering angular acceleration weight in matrix R", 0.0, 0.0, 10.0)
gen.add("mpc_weight_terminal_lat_error", double_t, 0, "terminal lateral error weight in matrix Q to improve mpc stability", 0.0, 0.0, 10.0)
gen.add("mpc_weight_terminal_heading_error", double_t, 0, "terminal heading error weight in matrix Q to improve mpc stability", 0.0, 0.0, 10.0)
gen.add("mpc_zero_ff_steer_deg", double_t, 0, "threshold that feed-forward angle becomes zero", 0.0, 0.0, 30.0)


# -- vehicle model --
gen.add("steer_lim_deg", double_t, 0, "steering angle limit [deg]", 40.0, 0.0, 90.0)
gen.add("steer_rate_lim_dps", double_t, 0, "steering angle rate limit [deg/s]", 600.0, 0.0, 1200.0)
gen.add("acceleration_limit", double_t, 0, "acceleration limit for trajectory velocity modification [m/ss]", 2.0, 0.01, 10.0)
gen.add("velocity_time_constant", double_t, 0, "velocity dynamics time constant  for trajectory velocity modification [s]", 0.3, 0.01, 10.0)
gen.add("input_delay", double_t, 0, "steering input delay time for delay compensation", 0.24, 0.0, 3.0)
# gen.add("vehicle_model_steer_tau", double_t, 0, "steering dynamics time constant (1d approximation) [s]", 0.3, 0.0, 3.0)
# gen.add("steering_lpf_cutoff_hz", double_t, 0, "cutoff frequency of lowpass filter for steering command [Hz]", 3.0, 0.0, 10.0)


exit(gen.generate(PACKAGE, "mpc_follower", "MPCFollower"))
